/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.targeting;

import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.ResolutionT;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.EllipseDescriptor;
import edu.wpi.first.wpilibj.image.EllipseMatch;
import edu.wpi.first.wpilibj.image.MonoImage;
import edu.wpi.first.wpilibj.image.NIVisionException;

/**
 *
 * @author FireBird Robotics
 */
public class Targeting {

    protected Points2D center;
    protected MonoImage m_Image;
    protected AxisCamera camera;
    protected EllipseDescriptor ellipseDescriptor;
    protected ResolutionT cameraResolution;

    protected int delta;

    public Targeting(AxisCamera camera, int delta, Object properties){
        this.camera = camera;
        this.ellipseDescriptor = new EllipseDescriptor(20, 100, 20, 100);
        this.delta = delta;
        this.cameraResolution = camera.getResolution();
    }

    public void run()
    {
        try {
            acquireImage();
            calculateCenter();
            releaseImage();
            nap();
        } catch (NIVisionException nivEx) {
            nivEx.printStackTrace();
        } catch (Exception ex) {
            ex.printStackTrace();
        }
    }

    protected void acquireImage() throws AxisCameraException, NIVisionException
    {
        ColorImage tmp = this.camera.getImage();
        this.m_Image = tmp.getLuminancePlane();
        tmp.free();
    }

    protected void releaseImage() throws NIVisionException
    {
        if (this.m_Image != null)
        {
            this.m_Image.free();
            this.m_Image = null;
        }

    }

    protected void nap()
    {

    }

    public synchronized int[] getCenter(){
            int[] newVal = new int[] {center.getX(), center.getY()};
            return newVal;
    }

    protected synchronized void calculateCenter() throws NIVisionException
    {
        EllipseMatch[] targets = m_Image.detectEllipses(this.ellipseDescriptor);

        System.out.println(targets.length + " : Number of targets");
//        for(int i = 0; i < targets.length; ++i)
//            System.out.println("Match " + i + " = " + targets[i].toString());


        if (targets.length < 2)
        {
            this.center.setX(-1);
            this.center.setY(-1);
        }
        else
        {
            EllipseMatch first  = targets[0];
            EllipseMatch second = targets[1];


            if ((Math.abs(first.m_xPos - second.m_xPos) < delta) &&
                (Math.abs(first.m_yPos - second.m_yPos) < delta))
            {
                this.center.setX((int) (first.m_xPos + second.m_xPos + .5) / 2);
                this.center.setY((int) (first.m_yPos + second.m_yPos + .5) / 2);
            }
        }
    }


    public static class Points2D{
        protected int x;
        protected int y;

        public Points2D(int x, int y){
            this.x = x;
            this.y = y;
        }

        public int getX(){
            return this.x;
        }

        public int getY(){
            return this.y;
        }

        protected void setX(int x){
            this.x = x;
        }

        protected void setY(int y){
            this.y = y;
        }

        public String toString(){
            return "(" + this.x + "," + this.y + ")";
        }
    }
}
